
#include "PhysicHandler.h"

void Critter::KinematicBody::setSceneNode(Ogre::SceneNode* node, Enums::SceneNodeDestructorBehaviour behavior){
	this->mNode = node;
}

PhysicHandler::PhysicHandler(){
	this->initNull();
}

PhysicHandler::PhysicHandler(OgreInitializeSettings *initsettings){
	this->initNull();
	this->ogre_init_settings = initsettings;
}

PhysicHandler::~PhysicHandler(){
	
	std::cout << "Destroying Physics \n";
	
	if(this->lRenderSystem != NULL){
		std::cout << "Destroying Critter \n";
	
		delete this->lRenderSystem;
	}

	if(this->lScene != NULL){
		std::cout << "Destroying Physics Scene\n";

		this->lWorld->destroyScene(this->lScene);
	}

	if(this->lWorld != NULL){
		std::cout << "Destroying Physics World\n";
		
		this->lWorld->destroyWorld(true);
	}
}

//init_methods

void PhysicHandler::initNull(){
	this->lScene = NULL;
	this->lWorld = NULL;
	this->lRenderSystem = NULL;
	this->ogre_init_settings = NULL;
}

void PhysicHandler::initBasic(Ogre::SceneManager* ogrescene){
	
	NxOgre::WorldDescription world_desc;
	NxOgre::SceneDescription scene_desc;

	this->lWorld = NxOgre::World::createWorld(world_desc);
	scene_desc.mUseHardware = false;
	scene_desc.mGravity = NxOgre::Vec3(0,-this->ogre_init_settings->gravity,0);
	
	if(this->ogre_init_settings->bounds != NULL){
		scene_desc.mBoundsPlane = true;
		
		NxOgre::Vec3 minvec (*this->ogre_init_settings->bounds);
		minvec = minvec*-1;
		NxOgre::Vec3 maxvec (*this->ogre_init_settings->bounds);

		NxOgre::Bounds3 bound (minvec,maxvec);
		scene_desc.mBounds.set(bound);
	}
	else{
		scene_desc.mBoundsPlane = false;
	}

	this->lScene = this->lWorld->createScene(scene_desc);
	
	this->lRenderSystem = new Critter::RenderSystem(this->lScene,ogrescene);
}

void PhysicHandler::init(Ogre::SceneManager* ogrescene){
	this->initBasic(ogrescene);
}

void PhysicHandler::update(unsigned long etime){

	float dtime = (float)etime;

	dtime = dtime;

	
	this->lWorld->advance(dtime);
	//this->lRenderSystem->advance(dtime,NxOgre::Enums::Priority_High,NxOgre::Enums::SceneFunction_Simulate);

	
}

void PhysicHandler::initTestScene(){


	NxOgre::MaterialDescription matdesc;
	matdesc.mName = "Test";
	matdesc.mRestitution = 0.1f;
	matdesc.mStaticFriction = 0.3f;
	matdesc.mDynamicFriction = 0.3f;
	

	NxOgre::MaterialIdentifier ident = this->lScene->createMaterial(matdesc)->getIdentifier();
	

//ground


	NxOgre::PlaneGeometryDescription help (NxOgre::Vec3(0,1,0),0,ident);
	
	this->lScene->createSceneGeometry(help);

	Ogre::Entity* ent1 = this->lRenderSystem->getSceneManager()->createEntity("box1","cube.mesh","FirstLoaded");
	ent1->setMaterialName("BaseWhite");
	float new_box_size1 = 1.0f/102.0f;
	Ogre::SceneNode* node = this->lRenderSystem->getSceneManager()->getRootSceneNode()->createChildSceneNode();
	node->attachObject(ent1);
	node->scale(new_box_size1*100,new_box_size1,new_box_size1*100);
	
	


//actor

	NxOgre::BoxDescription hellodesc2 (NxOgre::Vec3(5,5,5));
	hellodesc2.mMaterial = ident;
	Critter::BodyDescription heydesc2;
	heydesc2.mMass = 500;
	hellodesc2.mSkinWidth= 0.05f;
	
	Critter::Body* body2 = this->lRenderSystem->createBody(hellodesc2,NxOgre::Vec3(0,100,0.1f),heydesc2);
	body2->setSceneNode(this->lRenderSystem->getSceneManager()->getRootSceneNode()->createChildSceneNode());
	

	Ogre::Entity* ent2 = this->lRenderSystem->getSceneManager()->createEntity("box2","cube.mesh","FirstLoaded");
	ent2->setMaterialName("BaseWhite");
	float new_box_size2 = 5.0f/102.0f;
	body2->getSceneNode()->attachObject(ent2);
	body2->getSceneNode()->scale(new_box_size2,new_box_size2,new_box_size2);
	
	Critter::Body* body3 = this->lRenderSystem->createBody(hellodesc2,NxOgre::Vec3(0,50,0),heydesc2);
	body3->setSceneNode(this->lRenderSystem->getSceneManager()->getRootSceneNode()->createChildSceneNode());
	Ogre::Entity* ent3 = this->lRenderSystem->getSceneManager()->createEntity("box3","cube.mesh","FirstLoaded");
	ent3->setMaterialName("BaseWhite");
	body3->getSceneNode()->attachObject(ent3);
	body3->getSceneNode()->scale(new_box_size2,new_box_size2,new_box_size2);
	
	

	body3->setGlobalOrientationQuat(this->setOrientation(NxOgre::Vec3(0,0,45)));
	

}

NxOgre::Quat PhysicHandler::pitch(NxOgre::Quat in,float degrees){
	Ogre::Quaternion x (Ogre::Degree(degrees),Ogre::Vector3::UNIT_X);

	NxOgre::Quat q (x);
	NxOgre::Quat r (in);
	
	r = r*q;

	return (r);

}

NxOgre::Quat PhysicHandler::roll(NxOgre::Quat in,float degrees){
	Ogre::Quaternion x (Ogre::Degree(degrees),Ogre::Vector3::UNIT_Y);

	NxOgre::Quat q (x);
	NxOgre::Quat r (in);
	
	r = r*q;

	return (r);

}

NxOgre::Quat PhysicHandler::yaw(NxOgre::Quat in,float degrees){
	Ogre::Quaternion x (Ogre::Degree(degrees),Ogre::Vector3::UNIT_Z);

	NxOgre::Quat q (x);
	NxOgre::Quat r (in);
	
	r = r*q;

	return (r);

}

NxOgre::Quat PhysicHandler::setOrientation(NxOgre::Vec3 degrees){

	Ogre::Quaternion x (Ogre::Degree(degrees.x),Ogre::Vector3::UNIT_X);
	Ogre::Quaternion y (Ogre::Degree(degrees.y),Ogre::Vector3::UNIT_Y);
	Ogre::Quaternion z (Ogre::Degree(degrees.z),Ogre::Vector3::UNIT_Z);
	z = x*y*z;

	return NxOgre::Quat(z);
}